#include "integrationMethod.h"
#include <utility>
#include <cmath>
#include "face.h"
#include "defines/airType.h"
#define ERROR 0.00001

bool IntegrationMethod::ocilation = true;
unsigned int IntegrationMethod::ocilationTimes = 0;

IntegrationMethod::IntegrationMethod()
{
    deltaT = 0.0;
    useAir = false;
    airType = CONSTANT;
    airPeriod = 5;
    airRepeat = 3;
    airDirection = Vector3D(0, 0, 0);
    fluidViscosity = 0.0178;
    restart();
}

IntegrationMethod::IntegrationMethod(double _deltaT, double _g, std::vector<State> _s0,
                                     std::vector< std::vector<Particle *> > &_p, std::vector< std::vector<Spring *> > &_s,
                                     bool _activeAir, int _airType, double _airPeriod, double _airRepeat, double _airViscosity,
                                     Vector3D _airDirection)
                                     : deltaT(_deltaT), gravity(_g), currentState(_s0), previousState(_s0), particles (_p), springs(_s),
                                     useAir(_activeAir), airType(_airType), airPeriod(_airPeriod), airRepeat(_airRepeat),
                                     airDirection(_airDirection), fluidViscosity(_airViscosity)
{
    restart();
    //the initial state only needs to have the position and initial velocity
}

void IntegrationMethod::restart()
{
    ocilation = true;
    ocilationTimes = 0;
}

//time step
double IntegrationMethod::getDeltaT() const
{
    return deltaT;
}

//gravity
double IntegrationMethod::getGravity() const
{
    return gravity;
}

//position vector of current state
const Vector &IntegrationMethod::getPosition(unsigned int _i) const
{
    return (currentState.at(_i)).getPosition();
}

//velocity vector of current state
const Vector &IntegrationMethod::getVelocity(unsigned int _i) const
{
    return (currentState.at(_i)).getVelocity();
}

//return the current state of fabric in the position i
const State &IntegrationMethod::getCurrentState(unsigned int _i) const
{
    return currentState.at(_i);
}

//return the current state of fabric in the position i
const State &IntegrationMethod::getPreviousState(unsigned int _i) const
{
    return previousState.at(_i);
}

//modifies the previous and current states
void IntegrationMethod::setState(unsigned int _i, const State &_s)
{
    currentState.at(_i) = _s;

    previousState.at(_i) = _s;
}

//modifies the current state's velocity and position
//fabric , particle, newPosition and newVelocity
void IntegrationMethod::setCurrentStateInParticle(unsigned int _i, unsigned int _j, const Vector3D &_p, const Vector3D &_v)
{
    currentState.at(_i).setParticlePositionAndVelocity(_j, _p, _v);
}

void IntegrationMethod::setCurrentStateInParticle(unsigned int _i, unsigned int _j, bool _x, bool _y, bool _z, const Vector3D &_p, const Vector3D &_v)
{
    currentState.at(_i).setParticleState(_j, _x, _y, _z, _p, _v);
}

void IntegrationMethod::setCurrentState(unsigned int _i, const State &_s)
{

    currentState.at(_i) = _s;
}

//calculating the function F
State IntegrationMethod::f(const State &_state, unsigned int _i)
{
    Vector a = acceleration(_state, _i);

    return State (_state.getVelocity(), a);
}

//calculating the acceleration
Vector IntegrationMethod::acceleration(const State &_state, unsigned int _i)
{
    Matrix *m = populateM(_state, _i);

    Vector *rf = populateResultantForces(_state, _i);

    Vector acc = (*m).diagonalInverse() * (*rf);

    delete m;
    delete rf;
    return acc;
}

//calculating the vector of resultant force
Vector *IntegrationMethod::populateResultantForces(const State &_state, unsigned int _i, unsigned int _method)
{
    unsigned int dof = _state.getDOF();

    const Matrix &ref = _state.getReferenceMatrix();

    std::vector<Particle *> particlesI = particles.at(_i);

    std::vector<Spring *> springsI = springs.at(_i);


    //rf = fe + fd + fg + dragf + fc
    //gravitational force and collision force

    Vector gf(dof);

    Vector cf(dof);

    for(unsigned int i = 0; i < ref.getRow(); i++)
    {
        Particle *particle = particlesI.at(i);

        if (ref[i][2] != -1) gf[ref[i][2]] = particle->getMass() * gravity; //gravitacional force


        std::pair<bool, Object *> collision = _state.getCollision(i);

        if(collision.first)
        {
            Vector3D velocity, position;

            _state.getParticlePositionVelocity(i, particle->getPosition(), particle->getVelocity(), &position, &velocity);

            Vector3D normal = collision.second->getNormalAt(position);

            Vector3D normalComponent = normal*(velocity.dotProduct(normal));

            Vector3D tangentialComponent = velocity - normalComponent;

            for(int j = 0; j < 3; j++)
            {
                int place = ref[i][j];
                if (place != -1) cf[place] = (normalComponent[j] * particle->getMiC())
                                           + (tangentialComponent[j] * collision.second->getMi(velocity));
            }

        }
    }

    Vector ef = Vector(dof); //elastic force

    Vector df = Vector(dof); //damping force

    unsigned int size = springsI.size();

    //#pragma omp parallel for //schedule(dynamic)
    for(unsigned int i = 0; i < size; i++)
    {
        Spring *s = NULL;
        s = springsI.at(i);

        double lc;

        unsigned int begin, end;

        begin = s->getParticleBegin();

        end = s->getParticleEnd();

        Vector3D positionBegin, positionEnd;

        Vector3D velocityBegin, velocityEnd;

        Particle *particleBegin = particlesI.at(begin);

        Particle *particleEnd = particlesI.at(end);

        _state.getParticlePositionVelocity(begin, particleBegin->getPosition(), particleBegin->getVelocity(), &positionBegin, &velocityBegin);

        _state.getParticlePositionVelocity(end, particleEnd->getPosition(), particleEnd->getVelocity(), &positionEnd, &velocityEnd);

        lc = s->getCurrentLength(positionBegin, positionEnd);

        {
            //calculating the elastic force

            Vector3D force;

            double ks, lo;

            int currentDOF;

            bool delta;

            delta = s->positiveDelta();

            if(_method==KANG2004)
            {

                ks = s->getKs();

                lo = s->getOriginalLength();

                force = ( positionEnd - positionBegin ) * ( (ks * (lc - lo)) / lc );

                for(int j = 0; j < 3; j++)
                {
                    currentDOF = ref[s->getParticleBegin()][j];
                    if( currentDOF != -1 ) ef[currentDOF] += force[j];

                    currentDOF = ref[s->getParticleEnd()][j];
                    if( currentDOF != -1 ) ef[currentDOF] -= force[j];
                }

            }
            else if(_method==ZHOU2005)
            {
                if ((s->getType() == FLEXION && !delta) || (s->getType() != FLEXION))
                {
                    ks = s->getKs();

                    lo = s->getOriginalLength();

                    force = ( positionEnd - positionBegin ) * ( (ks * (lc - lo)) / lc );

                    for(int j = 0; j < 3; j++)
                    {
                        currentDOF = ref[s->getParticleBegin()][j];
                        if( currentDOF != -1 ) ef[currentDOF] += force[j];

                        currentDOF = ref[s->getParticleEnd()][j];
                        if( currentDOF != -1 ) ef[currentDOF] -= force[j];
                    }
                }
            }
            else if (_method == EBERHARDT2000)
            {

                if ((s->getType() == FLEXION && !delta) || (s->getType()==SHEAR))
                {
                    ks = s->getKs();

                    lo = s->getOriginalLength();

                    force = ( positionEnd - positionBegin ) * ( (ks * (lc - lo)) / lc );

                    for(int j = 0; j < 3; j++)
                    {
                        currentDOF = ref[s->getParticleBegin()][j];
                        if( currentDOF != -1 ) ef[currentDOF] += force[j];

                        currentDOF = ref[s->getParticleEnd()][j];
                        if( currentDOF != -1 ) ef[currentDOF] -= force[j];
                    }
                }
                else if(s->getType()==STRUCTURAL_HORIZONTAL || s->getType()==STRUCTURAL_VERTICAL)
                {

                    ks = s->getKs();

                    lo = s->getOriginalLength();

                    force = (positionEnd - positionBegin) * ( ((ks*lo)*-1.0) / lc );

                    for(int j = 0; j < 3; j++)
                    {
                        currentDOF = ref[s->getParticleBegin()][j];
                        if( currentDOF != -1 ) ef[currentDOF] += force[j];

                        currentDOF = ref[s->getParticleEnd()][j];
                        if( currentDOF != -1 ) ef[currentDOF] -= force[j];
                    }

                }
            }
            else
            {
                if ((s->getType() == FLEXION && !delta) || (s->getType() != FLEXION && delta))
                {
                    ks = s->getKs();

                    lo = s->getOriginalLength();

                    force = ( positionEnd - positionBegin ) * ( (ks * (lc - lo)) / lc );

                    for(int j = 0; j < 3; j++)
                    {
                        currentDOF = ref[s->getParticleBegin()][j];
                        if( currentDOF != -1 ) ef[currentDOF] += force[j];

                        currentDOF = ref[s->getParticleEnd()][j];
                        if( currentDOF != -1 ) ef[currentDOF] -= force[j];
                    }
                }
            }
        }

        {
            //calculating the damping force

            Vector3D force;

            int currentDOF;

            double kd;

            bool delta;

            delta = s->positiveDelta();

            if(_method == EBERHARDT2000)
            {
                if(s->getType()!=STRUCTURAL_HORIZONTAL && s->getType()!=STRUCTURAL_VERTICAL)
                {
                    kd = s->getKd();

                    //force = (velocityEnd - velocityBegin) * ( kd );
                    force = (positionEnd - positionBegin) * ((kd * ((velocityEnd - velocityBegin).dotProduct(positionEnd - positionBegin)))/ (lc * lc));
                     //std::cout<<"força de amortecimento ["<<force.x<<","<<force.y<<","<<force.z<<" ]" <<std::endl;

                    for(int j = 0; j < 3; j++)
                    {
                        currentDOF = ref[s->getParticleBegin()][j];
                        if( currentDOF != -1 ) df[currentDOF] += force[j];

                        currentDOF = ref[s->getParticleEnd()][j];
                        if( currentDOF != -1 ) df[currentDOF] -= force[j];
                    }
                }

            }
            else if(_method == KANG2004)
            {
                 kd = s->getKd();

                 //force = (velocityEnd - velocityBegin) * ( kd );
                 force = (positionEnd - positionBegin) * ((kd * ((velocityEnd - velocityBegin).dotProduct(positionEnd - positionBegin)))/ (lc * lc));
                 //std::cout<<"força de amortecimento ["<<force.x<<","<<force.y<<","<<force.z<<" ]" <<std::endl;

                 for(int j = 0; j < 3; j++)
                 {
                     currentDOF = ref[s->getParticleBegin()][j];
                     if( currentDOF != -1 ) df[currentDOF] += force[j];

                     currentDOF = ref[s->getParticleEnd()][j];
                     if( currentDOF != -1 ) df[currentDOF] -= force[j];
                 }


            }
             else if(_method == ZHOU2005)
            {
                if((s->getType() == FLEXION && !delta) || (s->getType() != FLEXION))
                {
                    kd = s->getKd();

                    //force = (velocityEnd - velocityBegin) * ( kd );
                    force = (positionEnd - positionBegin) * ((kd * ((velocityEnd - velocityBegin).dotProduct(positionEnd - positionBegin)))/ (lc * lc));
                     //std::cout<<"força de amortecimento ["<<force.x<<","<<force.y<<","<<force.z<<" ]" <<std::endl;

                    for(int j = 0; j < 3; j++)
                    {
                        currentDOF = ref[s->getParticleBegin()][j];
                        if( currentDOF != -1 ) df[currentDOF] += force[j];

                        currentDOF = ref[s->getParticleEnd()][j];
                        if( currentDOF != -1 ) df[currentDOF] -= force[j];
                    }
                }

            }
            else
            {
                if((s->getType() == FLEXION && !delta) || (s->getType() != FLEXION && delta))
                {

                    kd = s->getKd();

                    //force = (velocityEnd - velocityBegin) * ( kd );
                    force = (positionEnd - positionBegin) * ((kd * ((velocityEnd - velocityBegin).dotProduct(positionEnd - positionBegin))) / (lc * lc));
                    //std::cout<<"força de amortecimento ["<<force.x<<","<<force.y<<","<<force.z<<" ]" <<std::endl;

                    for(int j = 0; j < 3; j++)
                    {
                        currentDOF = ref[s->getParticleBegin()][j];
                        if( currentDOF != -1 ) df[currentDOF] += force[j];

                        currentDOF = ref[s->getParticleEnd()][j];
                        if( currentDOF != -1 ) df[currentDOF] -= force[j];
                    }
                }

            }

        }

    }

    Vector dragf = Vector(dof); //drag force of air

    Vector airf = Vector(dof); //force of wind

    {

        Vector3D fluidVelocity;
        if(useAir) fluidVelocity = velocityInTime(_state.getT());

        for(unsigned int k = 0; k < particlesI.size(); k++)
        {
            Particle *particle = particlesI.at(k);

            double kdParticle = particle->getKd();

            Vector3D velocityParticle = _state.getParticleVelocity(k, particle->getVelocity());

            Vector3D dragForce = velocityParticle * (kdParticle);

            Vector3D airForce;

            if(useAir)
            {
                Vector3D normal = _state.getParticleNormal(particle);

                airForce = normal * (fluidViscosity * (normal.dotProduct(fluidVelocity - velocityParticle)));
            }

            int currentDOF;

            for(int j = 0; j < 3; j++)
            {
                currentDOF = ref[k][j];
                if( currentDOF != -1 )
                {
                    dragf[currentDOF] -= dragForce[j];

                    if(useAir) airf[currentDOF] += airForce[j];
                }

            }


        }

    }

    Vector *rf = new Vector(dof);

    if(_method == KANG2000)
    {
        (*rf) = ef + (df.scalarProduct(currentDeltaT)) + gf + cf;
    }
    else
    {
        (*rf) =  ef + df + gf + dragf + cf;
    }

    if(useAir) (*rf) = (*rf) + airf;

    return rf;
}

Vector3D IntegrationMethod::velocityInTime(double t) const
{
    double k = 0;
    switch(airType)
    {
        case CONSTANT:
        {
            k = 1;
            break;
        }
        case SIN:
        {
            if(airPeriod != 0.0)
                if(ocilationTimes > 2*airRepeat)
                    return Vector3D(0.0, 0.0, 0.0);

            k = sin( (t/airPeriod) * 2 * M_PI * airRepeat);


            if(k > 0)//true
            {
                if(!ocilation)//mesmo sinal
                    ocilationTimes++;
                ocilation = true;
            }
            else{//false
                if(ocilation)//mesmo sinal
                    ocilationTimes++;
                ocilation = false;
                return Vector3D(0.0, 0.0, 0.0);
            }
            break;
        }
    }
    Vector3D v = airDirection * k;

    return airDirection * k;
}

//calculating the mass matrix
Matrix *IntegrationMethod::populateM(const State &_state, unsigned int _i)
{
    unsigned int dof = _state.getDOF();

    const Matrix &ref = _state.getReferenceMatrix();

    std::vector<Particle *> particlesI = particles.at(_i);

    Matrix *m = new Matrix(dof, dof);

    int k;

    for(unsigned int i = 0; i < ref.getRow(); i++)
    {
        for(int j = 0; j < 3; j++)
        {
            if (ref[i][j] != -1)
            {
                k = ref[i][j];
                (*m)[k][k] = particlesI.at(i)->getMass();
            }
        }
    }
    return m;
}

//calulando the decomposition Cholesky
Matrix IntegrationMethod::decompositionCholesky(const Matrix &_m)
{
    //Depois posso retirar esse teste se estiver tudo oK
    bool symmetric = _m.symmetricMatrix();

    if(!symmetric)
    {
        std::cout<<"Matrix nao e simetrica"<<std::endl;
        exit(1);
    }

    unsigned int r;
    r = _m.getRow();

    Matrix l(r, r);

    Matrix lt(r,r);

    l[0][0] = sqrt(_m[0][0]);

    for (unsigned int j=1; j<r;j++)
    {
        if (fabs(_m[j][0])<ERROR)
        {
            l[j][0]=0;
        }
        else
        {
            l[j][0]=(_m[j][0])/(l[0][0]);

        }

    }
    for (unsigned int i=1; i<(r-1);i++)
    {
        double sum = 0;

        for (unsigned int k=0; k<(i-1); k++)
        {
            sum = sum + (l[i][k]*l[i][k]);
        }

        l[i][i] = sqrt(_m[i][i] - sum);

        for(unsigned int j =(i+1); j<r; j++)
        {

            if (fabs(_m[j][0])<ERROR)
            {
                l[j][i]=0;
            }
            else
            {
                    double sum = 0;

                for (unsigned int k=0; k<(i-1); k++)
                {
                    sum = sum + (l[j][k]*l[i][k]);
                }
                l[j][i] = (_m[j][i]-sum)/l[i][i];

            }

        }

    }

    double total = 0;
    for(unsigned int k=0; k<(r-1); k++)
    {
        total = total + (l[r-1][k]*l[r-1][k]);
    }
    l[r-1][r-1] = sqrt(_m[r-1][r-1] - total);

    lt = l.transpose();
    Matrix result = l * lt;

    return result;
}

